using BepuUtilities;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints.Contact
{
    public struct PenetrationLimitOneBodyProjection
    {
        // 请注意,这些只是原始的雅克比,没有使用JT*EffectiveMass值进行预计算。
        public Vector3Wide AngularA;
        public Vector<float> EffectiveMass;
        public Vector<float> BiasVelocity;
    }

    public static class PenetrationLimitOneBody
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Prestep(in BodyInertias inertiaA,
            in Vector3Wide contactOffsetA, in Vector3Wide normal, in Vector<float> depth,
            in Vector<float> positionErrorToVelocity, in Vector<float> effectiveMassCFMScale, in Vector<float> maximumRecoveryVelocity,
            float inverseDt, out PenetrationLimitOneBodyProjection projection)
        {
            // 有关派生信息,请参见PenetgariLimit.cs。
            Vector3Wide.CrossWithoutOverlap(contactOffsetA, normal, out projection.AngularA);

            // 有效质量
            Symmetric3x3Wide.VectorSandwich(projection.AngularA, inertiaA.InverseInertiaTensor, out var angularA0);

            // 请注意,我们不预先计算JT*EffailtiveMass项。因为雅克比人是共用的,我们无论如何都要做繁殖。
            projection.EffectiveMass = effectiveMassCFMScale / (inertiaA.InverseMass + angularA0);

            // 如果深度为负,则偏移速度将允许向上移动,直到深度达到零。这之所以有效,是因为Position ErrorToVelocity*dt将始终<=1。
            projection.BiasVelocity = Vector.Min(
                depth * new Vector<float>(inverseDt),
                Vector.Min(depth * positionErrorToVelocity, maximumRecoveryVelocity));
        }


        /// <summary>
        /// 将冲量从约束空间变换到世界空间,并使用它修改身体的缓存世界空间速度。
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApplyImpulse(in PenetrationLimitOneBodyProjection projection, in BodyInertias inertiaA, in Vector3Wide normal,
            in Vector<float> correctiveImpulse, ref BodyVelocities wsvA)
        {
            var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass;
            Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity);
            Vector3Wide.Scale(projection.AngularA, correctiveImpulse, out var correctiveAngularImpulseA);
            Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity);

            Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear);
            Vector3Wide.Add(wsvA.Angular, correctiveVelocityAAngularVelocity, out wsvA.Angular);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void WarmStart(
            in PenetrationLimitOneBodyProjection projection, in BodyInertias inertiaA, in Vector3Wide normal,
            in Vector<float> accumulatedImpulse, ref BodyVelocities wsvA)
        {
            ApplyImpulse(projection, inertiaA, normal, accumulatedImpulse, ref wsvA);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeCorrectiveImpulse(in BodyVelocities wsvA,
            in PenetrationLimitOneBodyProjection projection,
            in Vector3Wide normal, in Vector<float> softnessImpulseScale,
            ref Vector<float> accumulatedImpulse, out Vector<float> correctiveCSI)
        {
            // 请注意,我们在这里没有使用预变换的雅克比;线性雅克比共享(法线)意味着我们无论如何都有有效质量。
            Vector3Wide.Dot(wsvA.Linear, normal, out var csvaLinear);
            Vector3Wide.Dot(wsvA.Angular, projection.AngularA, out var csvaAngular);
            // 计算求反版本,以避免需要显式求反。
            var negatedCSI = accumulatedImpulse * softnessImpulseScale + (csvaLinear + csvaAngular - projection.BiasVelocity) * projection.EffectiveMass;

            var previousAccumulated = accumulatedImpulse;
            accumulatedImpulse = Vector.Max(Vector<float>.Zero, accumulatedImpulse - negatedCSI);

            correctiveCSI = accumulatedImpulse - previousAccumulated;

        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Solve(in PenetrationLimitOneBodyProjection projection, in BodyInertias inertiaA, in Vector3Wide normal,
            in Vector<float> softnessImpulseScale, ref Vector<float> accumulatedImpulse, ref BodyVelocities wsvA)
        {
            ComputeCorrectiveImpulse(wsvA, projection, normal, softnessImpulseScale, ref accumulatedImpulse, out var correctiveCSI);
            ApplyImpulse(projection, inertiaA, normal, correctiveCSI, ref wsvA);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void UpdatePenetrationDepth(in Vector<float> dt, in Vector3Wide contactOffset, in Vector3Wide normal, in BodyVelocities velocity, ref Vector<float> penetrationDepth)
        {
            // 法线被校准为从B指向A。A沿N的任何移动都会导致深度减小。B沿N的任何移动都会导致深度的增加。
            // 但是一个实体约束没有B。
            // 估计深度深度变化=点(法线,速度DtA.线性+速度DtA.角度x接触偏移量A)
            Vector3Wide.CrossWithoutOverlap(velocity.Angular, contactOffset, out var wxr);
            Vector3Wide.Add(wxr, velocity.Linear, out var contactVelocity);
            Vector3Wide.Dot(normal, contactVelocity, out var estimatedDepthChange);
            penetrationDepth -= estimatedDepthChange * dt;
        }

    }
}
